/**
  ******************************************************************************
  * @file           : studio_plow_cover_ccpp.cpp
  * @author         : wangyingjie
  * @brief          : None
  * @attention      : None
  * @date           : 2025/6/8
  ******************************************************************************
  */


#include "studio_plow_cover_ccpp.h"
#include "geography/studio_proj.h"
#include "geometry/studio_vector_raster.h"


studio_line studio_plow_cover_ccpp::generate_path()
{
    // 如果不知道所有块的话 需要根据围栏框 生成栅格地图
    x_scan_line xlr_gs;
    if (all_blocks_.empty())
    {
        // 扫描线算法生成栅格地图
        double left = DBL_MAX, right = DBL_MIN, top = DBL_MIN, bottom = DBL_MAX;
        double gs_left = DBL_MAX, gs_right = DBL_MIN, gs_top = DBL_MIN, gs_bottom = DBL_MAX;
        if (fence_poly.outer_ring.points.empty())
        {
            std::cout << "fence_poly.outer_ring.points is empty!" << std::endl;
            optimized_path_.clear();
            return studio_line();
        }
        central = floor((fence_poly.outer_ring.points.begin()->x + 1.5) / 3.0) * 3.0;
        for (const auto& point : fence_poly.outer_ring.points)
        {
            double t_lon = point.x;
            double t_lat = point.y;
            left = std::min(left, t_lon);
            right = std::max(right, t_lon);
            top = std::max(top, t_lat);
            bottom = std::min(bottom, t_lat);
            // 经纬度转高斯
            double gx = 0.0, gy = 0.0;
            proj::lonlat_to_gauss(central, point.x, point.y, gx, gy);
            gs_fence_poly.outer_ring.points.push_back(studio_point(gx, gy));
            gs_left = std::min(gs_left, gx);
            gs_right = std::max(gs_right, gx);
            gs_top = std::max(gs_top, gy);
            gs_bottom = std::min(gs_bottom, gy);
        }

        studio_rect rect(gs_left, gs_top, gs_right, gs_bottom);
        double cell_size = cell_size_m;
        xlr_gs.set(rect, cell_size);
        xlr_gs.rasterize(gs_fence_poly);
        xlr_gs.fill();
        xlr_gs.blocks();
        all_blocks_ = xlr_gs.get_blocks();
    }

    // 步骤2: 按行蛇形遍历生成基础路径
    generate_snake_path();
    // 步骤3: 添加行间过渡点
    add_row_transitions();

    // 步骤4: 得到真是经纬路径
    studio_line gs_path;
    xlr_gs.get_grad_mid_path(optimized_path_, gs_path);
    // 转为经纬度路径
    studio_line gcs_path;
    for (const auto& point : gs_path)
    {
        double lon = 0.0, lat = 0.0;
        proj::gauss_to_lonlat(central, point.x, point.y, lon, lat);
        gcs_path.push_back(studio_point(lon, lat));
    }
    return gcs_path;
}


void studio_plow_cover_ccpp::generate_snake_path()
{
    base_path_.clear();
    bool left_to_right = true; // 起始方向：从左到右
    if (all_blocks_.empty())
    {
        std::cout << "all_blocks_ is empty!" << std::endl;
        return;
    }

    for (const auto& [row, cols] : all_blocks_)
    {
        std::vector<int> sorted_cols = cols; // 创建副本
        if (left_to_right)
        {
            if (!std::is_sorted(sorted_cols.begin(), sorted_cols.end()))
            {
                std::sort(sorted_cols.begin(), sorted_cols.end());
            }
        }
        else
        {
            if (!std::is_sorted(sorted_cols.rbegin(), sorted_cols.rend()))
            {
                std::sort(sorted_cols.begin(), sorted_cols.end(), std::greater<int>());
            }
        }

        // 添加当前行所有块到路径 <col, row>
        for (int col : sorted_cols)
        {
            base_path_.emplace_back(col, row);
        }

        // 切换到下一行时反转方向
        left_to_right = !left_to_right;
    }
}


void studio_plow_cover_ccpp::add_row_transitions()
{
    optimized_path_.clear();

    if (base_path_.empty())
        return;

    optimized_path_.push_back(base_path_[0]);

    for (size_t i = 1; i < base_path_.size(); i++)
    {
        const auto& prev = base_path_[i - 1];
        const auto& curr = base_path_[i];

        // 如果行号变化，添加垂直过渡点
        if (prev.second != curr.second)
        {
            int row_step = (curr.second > prev.second) ? 1 : -1;
            for (int r = prev.second + row_step; r != curr.second; r += row_step)
            {
                optimized_path_.emplace_back(prev.first, r); // <col, row>
            }
        }
        optimized_path_.push_back(curr);
    }
}